//
// Created by jyh on 2021/8/6.
//

#include "BaseStation.h"

int Sat_CountSat(bool isGLONASS){
    int i, nsat=0;
    if(!isGLONASS) {
        for (i = 0; i < BRDEPH.size_gnssEph; i++)
            if (BRDEPH.gnsseph[i].mjd != 0)
                nsat++;
    }else{
        for (i = 0; i < BRDEPH.size_glsEph; i++)
            if (BRDEPH.glseph[i].mjd != 0)
                nsat++;
    }
    return nsat;
}
int Sat_v_readOrbit(const char* cprn, int mjd, double sod, double* xsat, int* iode) {
    // using rinex navigation file to acquire orbit
    int wk, ret;
    double sow, tgd, dtmin, sclk;
    mjd2wksow(mjd, sod, &wk, &sow);
    memset(xsat, 0, sizeof(double) * 6);
    if (cprn[0] != 'R')
        if (cprn[0] == 'C')
            ret = BrdUtils_m_brd2xyz("yyy", cprn, wk, sow - 14.0, xsat, &sclk, &dtmin, &tgd, iode);
        else
            ret = BrdUtils_m_brd2xyz("yyy", cprn, wk, sow, xsat, &sclk, &dtmin, &tgd, iode);
    else
        ret = BrdUtils_m_gls2xyz("yyy", cprn, wk, sow, xsat, &sclk, &dtmin, iode);
    return ret;
}
char* Sat_m_getSatLocation(struct Station *sta) {
    double xsat_f[6], r1[6], r1leng, deltaDwe, tdelay;
    double geod[3], sod, sod_send, dump[3], ddelay;
    double rot_l2f[3][3], delay;
    int mjd, isat, jd_send, j, lorb;
    mjd = DLY.mjd;
    sod = DLY.sod;

    memset(sta->rleng, 0, sizeof(sta->rleng));
    xyzblh(sta->x, 1.0, 0, 0, 0, 0, 0, geod);
    //printf("%lf %lf %lf %lf %lf %lf\n", sta->x[0], sta->x[1], sta->x[2], geod[0], geod[1], geod[2]);
    rot_enu2xyz(geod[0], geod[1], rot_l2f);
    /*********** Initial control Variables ******/
    // Keep modeling
    for (isat = 0; isat < DLY.nprn; isat++)
    {
        ddelay = 0.1;
        delay = 0;
        sta->iode[isat] = -2; /* initilize the iode here */
        int itr=0;
        while (fabs(ddelay) > 1e-9)
        {
            timinc(mjd, sod, -delay, &jd_send, &sod_send);
            if (!(lorb = Sat_v_readOrbit(DLY.cprn[isat], jd_send, sod_send, xsat_f, sta->iode[isat])))
            {
                break;
            }
            // geometric distance
            for (j = 0; j < 3; j++)
            {
                r1[j] = xsat_f[j] - sta->x[j];
            }
            r1leng = sqrt(dot(3, r1, r1));
            // earth rotation correction
            deltaDwe = E_ROTATE / VEL_LIGHT * (xsat_f[1] * (xsat_f[0] - sta->x[0]) - xsat_f[0] * (xsat_f[1] - sta->x[1]));
            r1leng += deltaDwe;
            tdelay = r1leng / VEL_LIGHT;
            ddelay = tdelay - delay;
            delay = tdelay;
            if(itr++>10) break;
        }
        if (!lorb)
        {
            sta->rleng[isat] = 0.0;
            continue;
        }else if (itr>10)
        {
            printf("*****WARNING(BaseStation.c): %s position not convergence! ddelay=%14.13lf\n", DLY.cprn[isat], ddelay);
            sta->rleng[isat] = 0.0;
            continue;
        }
        sta->rleng[isat] = r1leng;
        matmpy(r1, (double*)rot_l2f, dump, 1, 3, 3);
        sta->azim[isat] = atan2(dump[0], dump[1]);
        sta->elev[isat] = atan(dump[2] / sqrt(dump[0] * dump[0] + dump[1] * dump[1]));
    }

    // create sat location buff and return it.
    char *satbuf=calloc(30*MAXSAT,sizeof(char));
    char *tmpbuf=calloc(30,sizeof(char));
    for(isat=0;isat<DLY.nprn;isat++){
        //sta->elev[isat]=1+isat/180.0*pi;
        //sta->azim[isat]=1+isat/180.0*pi*1.5;
        if(strcmp(DLY.cprn[isat],"")==0)
            continue;
        if (fabs(sta->azim[isat])<1e-9 && fabs(sta->elev[isat])<1e-9)
            continue;
        sprintf(tmpbuf,"%s %.2lf %.2lf\n",DLY.cprn[isat],sta->elev[isat]/pi*180.0,sta->azim[isat]/pi*180.0);
        strcat(satbuf,tmpbuf);
    }
    free(tmpbuf);
    return satbuf;
}
